/*******************************************************************************
* Copyright (c) 2008, Jeferson Jose de Miranda                                 *
* UDESC - Universidade do Estado de Santa Catarina                             *
* All rights reserved.                                                         *
*                                                                              *
* Redistribution and use in source and binary forms, with or without           *
* modification, are permitted provided that the following conditions are met:  *
*     * Redistributions of source code must retain the above copyright         *
*       notice, this list of conditions and the following disclaimer.          *
*     * Redistributions in binary form must reproduce the above copyright      *
*       notice, this list of conditions and the following disclaimer in the    *
*       documentation and/or other materials provided with the distribution.   *
*     * Neither the name of the Universidade do Estado de Santa Catarina nor   *
*       the names of its contributors may be used to endorse or promote        *
*       products derived from this software without specific prior written     *
*       permission.                                                            *
*                                                                              *
* THIS SOFTWARE IS PROVIDED BY JEFERSON JOSE DE MIRANDA "AS IS" AND ANY        *
* EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED    *
* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE       *
* DISCLAIMED. IN NO EVENT SHALL JEFERSON JOSE DE MIRANDA BE LIABLE FOR ANY     *
* DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES   *
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; *
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND  *
* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT   *
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF     *
* THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.            *
*******************************************************************************/

/**
 * \file FlowVectors.hpp
 *
 * \brief A sequence of 2D points.
 *
 * \author Jeferson Jose de Miranda
 */

#ifndef FLOWVECTORS_HPP
#define FLOWVECTORS_HPP

// OpenCV main header.
#include <cv.h>
// Operations between OpenCV's 2D points (CvPoint).
#include "Point2DMath.hpp"
// Exceptions.hpp groups Move-In exception messages.
#include "Exceptions.hpp"

namespace MoveIn
{
    /**
    * \class FlowVectors
    *
    * \brief This is an array of 2D u points, related to the result of
    * optical algorithm.
    */
    class FlowVectors
    {
        private:
        CvSeq *start;
        CvSeq *end;
        CvMemStorage *start_storage;
        CvMemStorage *end_storage;
        CvPoint origin;

        public:
        FlowVectors();

        ~FlowVectors();

        void append(CvPoint s,
                    CvPoint e);

        void insert(CvPoint s,
                    CvPoint e,
                    unsigned int index);

        void remove(unsigned int index);

        CvPoint getStart(int index);

        CvPoint getEnd(int index);

        unsigned int size();

        void reset();

        bool equals(CvPoint s,
                    CvPoint e,
                    unsigned int index);

        int realIndex(int index);

        void clone(FlowVectors *ps);

        void translate(int x,
                       int y);

        void print(RGBImage *source, float scalar_multiplicator = 3.0);

        void setOrigin(CvPoint origin);

        void sum();

        void result();

        CvPoint getTranslationVector();
    };

/*******************************************************************************
* MoveIn::FlowVectors::FlowVectors                                             *
*******************************************************************************/

    FlowVectors::FlowVectors()
    {
        start_storage = NULL;
        start_storage = cvCreateMemStorage(0);
        if(start_storage == NULL)
        {
            throw new OutOfMemory();
        }
        else
        {
            start = cvCreateSeq(CV_32SC2,
                                sizeof(*start),
                                sizeof(CvPoint),
                                start_storage);
        }
        end_storage = NULL;
        end_storage = cvCreateMemStorage(0);
        if(end_storage == NULL)
        {
            throw new OutOfMemory();
        }
        else
        {
            end = cvCreateSeq(CV_32SC2,
                              sizeof(*end),
                              sizeof(CvPoint),
                              end_storage);
        }
    }

/*******************************************************************************
* MoveIn::FlowVectors::~FlowVectors                                            *
*******************************************************************************/

    FlowVectors::~FlowVectors()
    {
        cvClearSeq(start);
        cvReleaseMemStorage(&start_storage);
        cvClearSeq(end);
        cvReleaseMemStorage(&end_storage);
    }

/*******************************************************************************
* MoveIn::FlowVectors::append                                                  *
*******************************************************************************/

    void FlowVectors::append(CvPoint s,
                             CvPoint e)
    {
        cvSeqPush(start, &s);
        cvSeqPush(end, &e);
    }


/*******************************************************************************
* MoveIn::FlowVectors::insert                                                  *
*******************************************************************************/

    void FlowVectors::insert(CvPoint s,
                             CvPoint e,
                             unsigned int index)
    {
        if(index > (unsigned int) start->total)
        {
            throw new OutOfBounds();
        }
        else
        {
            cvSeqInsert(start, index, &s);
            cvSeqInsert(end, index, &e);
        }
    }

/*******************************************************************************
* MoveIn::FlowVectors::remove                                                  *
*******************************************************************************/

    void FlowVectors::remove(unsigned int index)
    {
        if(index >= (unsigned int) start->total)
        {
            throw new OutOfBounds();
        }
        else
        {
            cvSeqRemove(start, index);
            cvSeqRemove(end, index);
        }
    }

/*******************************************************************************
* MoveIn::FlowVectors::getStart                                                *
*******************************************************************************/

    CvPoint FlowVectors::getStart(int index)
    {
        try
        {
            int idx = realIndex(index);
            CvPoint *p = (CvPoint*) cvGetSeqElem(start, idx);
            return cvPoint(p->x, p->y);
        }
        catch(std::exception *e)
        {
            throw e;
        }
    }

/*******************************************************************************
* MoveIn::FlowVectors::getEnd                                                  *
*******************************************************************************/

    CvPoint FlowVectors::getEnd(int index)
    {
        try
        {
            int idx = realIndex(index);
            CvPoint *p = (CvPoint*) cvGetSeqElem(end, idx);
            return cvPoint(p->x, p->y);
        }
        catch(std::exception *e)
        {
            throw e;
        }
    }

/*******************************************************************************
* MoveIn::FlowVectors::size                                                    *
*******************************************************************************/

    unsigned int FlowVectors::size()
    {
        return start->total;
    }

/*******************************************************************************
* MoveIn::FlowVectors::reset                                                   *
*******************************************************************************/

    void FlowVectors::reset()
    {
        cvClearSeq(start);
        cvClearSeq(end);
    }

/*******************************************************************************
* MoveIn::FlowVectors::equals                                                  *
*******************************************************************************/

    bool FlowVectors::equals(CvPoint s,
                             CvPoint e,
                             unsigned int index)
    {
        if(index >= (unsigned int) start->total)
        {
            throw new OutOfBounds();
        }
        else
        {
            CvPoint pts, pte;
            pts = getStart(index);
            pte = getEnd(index);
            if(s.x == pts.x && s.y == pts.y &&
               e.x == pte.x && e.y == pte.y)
            {
                return true;
            }
            else
            {
                return false;
            }
        }
    }

/*******************************************************************************
* MoveIn::FlowVectors::realIndex                                               *
*******************************************************************************/

    int FlowVectors::realIndex(int index)
    {
        if(size() == 0)
        {
            return -1;
        }
        else if((index % (int) size()) == 0)
        {
            return 0;
        }
        else if(index < 0)
        {
            return size() + (index % (int) size());
        }
        else if(index >= (int) size())
        {
            return index % size();
        }
        else
        {
            return index;
        }
    }

/*******************************************************************************
* MoveIn::FlowVectors::clone                                                   *
*******************************************************************************/

    void FlowVectors::clone(FlowVectors *fv)
    {
        reset();
        for(unsigned int i=0; i<fv->size(); i++)
        {
            append(fv->getStart(i), fv->getEnd(i));
        }
    }

/*******************************************************************************
* MoveIn::FlowVectors::translate                                               *
*******************************************************************************/

    void FlowVectors::translate(int x,
                                int y)
    {
        CvPoint pts, pte;
        for(unsigned int i=0; i<size(); i++)
        {
            pts = getStart(i);
            pts.x += x;
            pts.y += y;
            pte = getEnd(i);
            pte.x += x;
            pte.y += y;
            remove(i);
            insert(pts, pte, i);
        }
    }

/*******************************************************************************
* MoveIn::FlowVectors::print                                                   *
*******************************************************************************/

    void FlowVectors::print(RGBImage *source, float scalar_multiplicator)
    {
        // For fun and debugging, draw the flow field.
        for(unsigned int i=0; i<size(); i++)
        {
            // Make the flow field look nice with arrows.
            // The arrows will may be a bit too short for a nice visualization
            // because of the framerate, i.e., there's not much motion between
            // the frames. So, they wiil be lengthened by a factor of 3.
            CvPoint p, q;
            p = getStart(i);
            q = getEnd(i);
            double angle, hypotenuse;
            angle = atan2((double) p.y - q.y, (double) p.x - q.x);
            hypotenuse = sqrt(pow((p.y - q.y), 2.0) + pow((p.x - q.x), 2.0));
            // Lengthen the arrow according to scalar multiplicator provided.
            q.x =
            (int) (p.x - scalar_multiplicator * hypotenuse * cos(angle));
            q.y =
            (int) (p.y - scalar_multiplicator * hypotenuse * sin(angle));
            Point2DMath::printVector(source, p, q);
        }
    }

/*******************************************************************************
* MoveIn::FlowVectors::setOrigin                                               *
*******************************************************************************/

    void FlowVectors::setOrigin(CvPoint origin)
    {
        this->origin = origin;
    }

/*******************************************************************************
* MoveIn::FlowVectors::sum                                                     *
*******************************************************************************/

    void FlowVectors::sum()
    {
        CvPoint u, v;
        if(size() == 0)
        {
            return;
        }
        // Get the first vector.
        u = getEnd(0);
        // Translate vector to origin (0, 0).
        u.x -= getStart(0).x;
        u.y -= getStart(0).y;
        for(unsigned int i=1; i<size(); i++)
        {
            // Get the ith vector.
            v = getEnd(i);
            // Translate ith vector to origin (0, 0).
            v.x -= getStart(i).x;
            v.y -= getStart(i).y;
            // Sum vectors.
            u = Point2DMath::sum(u, v);
        }
        // Translate result to ROI origin (ROI middle point).
        u.x += origin.x;
        u.y += origin.y;
        // Reset vector list.
        reset();
        // Add result to list.
        append(origin, u);
    }

/*******************************************************************************
* MoveIn::FlowVectors::result                                                  *
*******************************************************************************/

    void FlowVectors::result()
    {
        CvPoint u;
        CvPoint v = cvPoint(0, 0);
        float magnitude = 0.0;
        if(size() == 0)
        {
            return;
        }
        for(unsigned int i=0; i<size(); i++)
        {
            u.x = (getStart(i).x - getEnd(i).x);
            u.y = (getStart(i).y - getEnd(i).y);
            v.x += u.x;
            v.y += u.y;
            magnitude += Point2DMath::magnitude(u);
        }
        magnitude /= size();
        double angle, hypotenuse;
        angle = atan2((double) u.y - v.y, (double) u.x - v.x);
        hypotenuse = sqrt(pow((u.y - v.y), 2.0) + pow((u.x - v.x), 2.0));
        v.x = (int) (magnitude * cos(angle));
        v.y = (int) (magnitude * sin(angle));
        // Translate result to ROI origin (ROI middle point).
        v.x += origin.x;
        v.y += origin.y;
        // Reset vector list.
        reset();
        // Add result to list.
        append(origin, v);
    }

/*******************************************************************************
* MoveIn::FlowVectors::getTranslationVector                                    *
*******************************************************************************/

    CvPoint FlowVectors::getTranslationVector()
    {
        CvPoint s = cvPoint(0, 0);
        CvPoint e = cvPoint(0, 0);
        if(size() == 0)
        {
            return s;
        }
        for(unsigned int i=0; i<size(); i++)
        {
            s.x += getStart(i).x;
            s.y += getStart(i).y;
            e.x += getEnd(i).x;
            e.y += getEnd(i).y;
        }
        s.x /= size();
        s.y /= size();
        e.x /= size();
        e.y /= size();

        e.x -= s.x;
        e.y -= s.y;

        return e;
    }
}

#endif
